/*******************************************************************************
* Copyright 2017 CLNST CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.

*******************************************************************************/
#include <rocr6_moveit_driver/rocr6-ctrl.h>

#define TIMEOUT_SEC(buflen,baud) (buflen*20/baud+2)             //接收超时
#define TIMEOUT_USEC 0

#define BUFFER_SIZE     18
#define DATA_GAIN           10

typedef struct{
    int         baudrate;                                       //baudrate
    int         databit;                                        //data bits, 5, 6, 7, 8
    int         fctl;                                           //flow control, 0: none, 1: hardware, 2: software
    int         parity;                                         //parity 0: none, 1: odd, 2: even
    int         stopbit;                                        //stop bits, 1, 2
    const int   reserved;                                       //reserved, must be zero
}PortInfo_t;

typedef PortInfo_t *pPortInfo_t;

static int fdcom = 0;

static unsigned char sendbuf[BUFFER_SIZE];
static unsigned char recvbuf[BUFFER_SIZE];

/*******************************************
 *  波特率转换函数（请确认是否正确）
********************************************/
static int convBaudRate(unsigned long int baudrate)
{
    switch(baudrate){
        case 2400:
            return B2400;
        case 4800:
            return B4800;
        case 9600:
            return B9600;
        case 19200:
            return B19200;
        case 38400:
            return B38400;
        case 57600:
            return B57600;
        case 115200:
            return B115200;
        default:
            return B9600;
    }
}

/*******************************************
 *  Setup serial attr
 *  fdcom: 串口文件描述符，pportinfo: 待设置的端口信息（请确认）
 *
********************************************/
static int uart_set(int fdcom, const pPortInfo_t pportinfo)
{
    struct termios termios_old, termios_new;
    int     baudrate, tmp;
    char    databit, stopbit, parity, fctl;

    bzero(&termios_old, sizeof(termios_old));
    bzero(&termios_new, sizeof(termios_new));
    cfmakeraw(&termios_new);
    tcgetattr(fdcom, &termios_old);                                 	//get the serial port attributions
    /*------------设置端口属性----------------*/
    //baudrates
    baudrate = convBaudRate(pportinfo -> baudrate);
    cfsetispeed(&termios_new, baudrate);                            	//填入串口输入端的波特率
    cfsetospeed(&termios_new, baudrate);                            	//填入串口输出端的波特率
    termios_new.c_cflag |= CLOCAL;                                  	//控制模式，保证程序不会成为端口的占有者
    termios_new.c_cflag |= CREAD;                                   	//控制模式，使能端口读取输入的数据

    // 控制模式，flow control
    fctl = pportinfo-> fctl;
    switch(fctl){
        case 0:{
            termios_new.c_cflag &= ~CRTSCTS;                        	//no flow control
        }break;
        case 1:{
            termios_new.c_cflag |= CRTSCTS;                         	//hardware flow control
        }break;
        case 2:{
            termios_new.c_iflag |= IXON | IXOFF |IXANY;             	//software flow control
        }break;
    }

    //控制模式，data bits
    termios_new.c_cflag &= ~CSIZE;                                  	//控制模式，屏蔽字符大小位
    databit = pportinfo -> databit;
    switch(databit){
        case 5:
            termios_new.c_cflag |= CS5;
        case 6:
            termios_new.c_cflag |= CS6;
        case 7:
            termios_new.c_cflag |= CS7;
        default:
            termios_new.c_cflag |= CS8;
    }

    //控制模式 parity check
    parity = pportinfo -> parity;
    switch(parity){
        case 0:{
            termios_new.c_cflag &= ~PARENB;                         	//no parity check
        }break;
        case 1:{
            termios_new.c_cflag |= PARENB;                          	//odd check
            termios_new.c_cflag &= ~PARODD;
        }break;
        case 2:{
            termios_new.c_cflag |= PARENB;                          	//even check
            termios_new.c_cflag |= PARODD;
        }break;
    }

    //控制模式，stop bits
    stopbit = pportinfo -> stopbit;
    if(stopbit == 2){
        termios_new.c_cflag |= CSTOPB;                              	//2 stop bits
    }
    else{
        termios_new.c_cflag &= ~CSTOPB;                             	//1 stop bits
    }

    //other attributions default
    termios_new.c_oflag &= ~OPOST;          				            //输出模式，原始数据输出
    termios_new.c_cc[VMIN]  = 1;            				            //控制字符, 所要读取字符的最小数量
    termios_new.c_cc[VTIME] = 1;            				            //控制字符, 读取第一个字符的等待时间	unit: (1/10)second

    //tcflush(fdcom, TCIFLUSH);               				            //溢出的数据可以接收，但不读
    tcflush(fdcom, TCIOFLUSH);  
    tmp = tcsetattr(fdcom, TCSANOW, &termios_new);  			        //设置新属性，TCSANOW：所有改变立即生效	tcgetattr(fdcom, &termios_old);

    return(tmp);
}

/*******************************************
 *  Open serial port
 *  tty: 端口号 ttyS0, ttyS1, ....
 *  返回值为串口文件描述符
********************************************/
static int uart_init(const char* dev)
{
    return(open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK));
}

/*******************************************
 *  Close serial port
********************************************/
static void uart_deinit(int fdcom)
{
    close(fdcom);
}

/********************************************
 *  send data
 *  fdcom: 串口描述符，data: 待发送数据，datalen: 数据长度
 *  返回实际发送长度
*********************************************/
static int uart_txd(int fdcom, const unsigned char *data, int datalen)
{
    int len = 0;

    len = write(fdcom, data, datalen);
    if(len == datalen){
        return (len);
    }
    else{
        tcflush(fdcom, TCOFLUSH);
        return -1;
    }
}

/*******************************************
 *  receive data
 *  返回实际读入的字节数
 *
********************************************/
static int uart_rxd(int fdcom, unsigned char *data, int datalen, int baudrate)
{
    int readlen, fs_sel;

    fd_set  fs_read;
    struct timeval tv_timeout;

    FD_ZERO(&fs_read);
    FD_SET(fdcom, &fs_read);
    tv_timeout.tv_sec = TIMEOUT_SEC(datalen, baudrate);
    tv_timeout.tv_usec = TIMEOUT_USEC;

    fs_sel = select(fdcom+1, &fs_read, NULL, NULL, &tv_timeout);
    if(fs_sel){
        readlen = read(fdcom, data, datalen);
        return(readlen);
    }
    else{
        return (-1);
    }

    return (readlen);
}

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                                    机械臂扩展接口
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static double msg[6] = {1, 0, 0, 0, 0, 0}; 

/**
  * @brief  数据帧校验
  * @param  消息数据
  * @retval 校验结果
  */
static int uart_cksm(const unsigned char buf[18])
{
	int tmp = 0;
	int cs = 0;
	int i;
	for (i = 0; i < 16; i += 2)
	{
		tmp = buf[i + 1];
		tmp <<= 8;
		tmp += buf[i];
		cs += tmp;
	}
	tmp = buf[17];
	tmp <<= 8;
	tmp += buf[16];
	if ((unsigned short)cs == tmp)
	{
		return 1;
	}
	return 0;
}

/**
  * @brief  机械臂发送数据转换
  * @param  输入弧度
  * @retval 变换后的角度
  */
double rocr6_tf_tdata(double dt)
{
  return dt*DATA_GAIN*180/PI;
}

/**
  * @brief  机械臂接收数据转换
  * @param  输入角度
  * @retval 变换后的弧度
  */
 double rocr6_tf_rdata(double dt)
 {
   return dt/DATA_GAIN*PI/180;
 }

/**
  * @brief  机械臂发送角度数据
  * @param  命令，参数，角度值
  * @retval 实际发送的数据
  */
int rocr6_txSig(unsigned short cmd, unsigned short index, const double dt[6])
{
  short t_data;
	int i, j=0;
	unsigned int cs;
	sendbuf[0] = (unsigned char)cmd;
	sendbuf[1] = (unsigned char)(cmd >> 8u);
	cs = cmd;
	sendbuf[2] = (unsigned char)index;
	sendbuf[3] = (unsigned char)(index >> 8u);
	cs += index;

	for (i = 0; i < 12; i += 2)
	{
		t_data = (short)dt[j];
		cs += (unsigned short)t_data;
		sendbuf[i+4] = (unsigned char)t_data;
		sendbuf[i+5] = (unsigned char)(t_data >> 8u);
		j++;
	}

	sendbuf[16] = (unsigned char)cs;
  sendbuf[17] = (unsigned char)(cs >> 8u);

  // printf("txd:[ ");
  // for(int k=0; k<18;k++)
  // {
  //   printf("0x%02x ",sendbuf[k]);
  // }
  // printf("\r\n");

  return uart_txd(fdcom, sendbuf, BUFFER_SIZE);
}

/**
  * @brief  机械臂接收角度数据
  * @param  接收到的角度
  * @retval 机械臂运行状态
  */
int rocr6_rxSig(double dt[6])
{
  short t_data;
  short cmd = 0;
  short flag = 0;
  int i, j = 0;

  if(uart_rxd(fdcom, recvbuf, BUFFER_SIZE, 115200))
  {
    if(uart_cksm(recvbuf))
    {
      t_data = recvbuf[1];
		  t_data <<= 8;
	    t_data += recvbuf[0];
	    cmd = t_data;
      // printf("rxd:[ ");
      // for(int k=0; k<18;k++)
      // {
      //     printf("0x%02x ",recvbuf[k]);
      // }
      // printf("\r\n");
      if(cmd==0x5a)
      {
        t_data = recvbuf[3];
		    t_data <<= 8;
	      t_data += recvbuf[2];
	      flag = t_data;
	      for (i = 0; i < 12; i += 2)
		    {
		      t_data = recvbuf[i + 5];
		      t_data <<= 8;;
			    t_data += recvbuf[i + 4];
			    dt[j] = t_data;
			    j++;
		    }
      }
    }
  }
  return flag;
}

/**
  * @brief  机械臂抱闸设置
  * @param  无
  * @retval 无
  */
void rocr6_brake(unsigned char flag)
{
  msg[1] = flag%2;
  rocr6_txSig(0x80,0,msg); 
  usleep(10);
}

/**
  * @brief  机械臂广播设置
  * @param  无
  * @retval 无
  */
void rocr6_broadcast(unsigned char flag)
{
  msg[1] = flag%2;
  rocr6_txSig(0xff,0,msg); 
  usleep(10);
}

/**
  * @brief  机械臂回HOME位置
  * @param  无
  * @retval 无
  */
void rocr6_home_config(void)
{
  rocr6_txSig(1,0,msg);
  printf("机械臂回到起始位置\r\n");
  usleep(10);
}

/**
  * @brief  启动机械臂连接
  * @param  无
  * @retval 初始化结果
  */
int rocr6_init(const char * dev)
{
  fdcom = uart_init(dev);
  if(fdcom > 0)
  {
    PortInfo_t portinfo = {115200, 8, 2, 0, 1, 0};
    uart_set(fdcom, &portinfo);
    printf("开启机械臂通信\r\n");
    return 1;
  }
  else
  {
    printf("[ERROR]: open port error.\n");
  }
  return -1;
}

/**
  * @brief 断开机械臂连接
  * @param  无
  * @retval 无
  */
void rocr6_deinit(void)
{
  if(fdcom)
  {
    printf("关闭机械臂通信\r\n");
    uart_deinit(fdcom);
  }
}

/************************ (C) COPYRIGHT HopeMotion *****END OF FILE****/

